/*
 * Copyright (c) 2010, Jim Hollinger
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimer in the
 *     documentation and/or other materials provided with the distribution.
 *   * Neither the name of Jim Hollinger nor the names of its contributors
 *     may be used to endorse or promote products derived from this
 *     software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */

#include "debughelper/debughelper.h"
#include "dtime/dtime.h"
#include "logger/logger.h"
#include "nmeainterface.h"
#include "nmeastatus.h"
#include "nmeaopenjauscomponent.h"


using namespace nmeajgp;


JausBoolean jausTimeSetTime(JausTime jausTime, time_t timeVal,
                            JausUnsignedShort millisec);

static void nmeajgp::processMessage_callback(OjCmpt cmpt, JausMessage rxMessage) {
    if (cmpt != NULL) {
        NmeaOpenJausComponent *object =
            reinterpret_cast<NmeaOpenJausComponent *> (ojCmptGetUserData(cmpt));
        if (object != NULL) {
            object->processMessage(cmpt, rxMessage);
        }
    }
}

static void nmeajgp::processState_callback(OjCmpt cmpt) {
    if (cmpt != NULL) {
        NmeaOpenJausComponent *object =
            reinterpret_cast<NmeaOpenJausComponent *> (ojCmptGetUserData(cmpt));
        if (object != NULL) {
            object->processState(cmpt);
        }
    }
}

NmeaOpenJausComponent::NmeaOpenJausComponent(NmeaInterface *theInterface,
                                             logger::Logger *logger)
    : jausComponent(NULL), nmeaInterface(theInterface),
      nmeaStatus(NULL), log(logger) {

    // create the nmea status object
    nmeaStatus = new NmeaStatus();
}

NmeaOpenJausComponent::~NmeaOpenJausComponent() {
    stop();

    if (jausComponent != NULL) {
        ojCmptDestroy(jausComponent);
        jausComponent = NULL;
    }

    delete nmeaStatus;
    nmeaStatus = NULL;
}

bool NmeaOpenJausComponent::init(double componentRate_hz) {
    bool flag = false;

    if ((nmeaInterface == NULL) || (nmeaStatus == NULL)) {
        log->fatal("NmeaOpenJausComponent.init: All objects not created");
    } else {

        // create jaus component
        JausByte component_ID = JAUS_GLOBAL_POSE_SENSOR;
        jausComponent =
            ojCmptCreate("NMEA Global Pose", component_ID, componentRate_hz);
        if (jausComponent != NULL) {
            ojCmptSetUserData(jausComponent, this);

            // add services of this component
            ojCmptAddService(jausComponent, component_ID);
            ojCmptAddServiceInputMessage(jausComponent, component_ID,
                JAUS_QUERY_GLOBAL_POSE,  JAUS_BYTE_PRESENCE_VECTOR_ALL_ON);
            ojCmptAddServiceOutputMessage(jausComponent, component_ID,
                JAUS_REPORT_GLOBAL_POSE, JAUS_BYTE_PRESENCE_VECTOR_ALL_ON);
            ojCmptAddSupportedSc(jausComponent, JAUS_REPORT_GLOBAL_POSE);

            // add component callbacks
            ojCmptSetMessageProcessorCallback(jausComponent,
                processMessage_callback);
            ojCmptSetStateCallback(jausComponent,
                JAUS_READY_STATE, processState_callback);

            flag = true;
        } else {
            log->fatal("NmeaOpenJausComponent: Node Manager not running");
        }
    }

    return flag;
}

bool NmeaOpenJausComponent::start() {
    bool flag = false;

    // set initial state and start the component
    if (jausComponent != NULL) {
        ojCmptSetState(jausComponent, JAUS_READY_STATE);
        if (ojCmptRun(jausComponent) >= 0) {
            log->info("NmeaOpenJausComponent started");
            flag = true;
        } else {
            log->fatal("NmeaOpenJausComponent.start: ojCmptRun failed");
        }
    }

    return flag;
}

void NmeaOpenJausComponent::stop() {
    if (jausComponent != NULL) {
        ojCmptRemoveSupportedSc(jausComponent, JAUS_REPORT_GLOBAL_POSE);
    }
}

void NmeaOpenJausComponent::processMessage(OjCmpt cmpt, JausMessage rxMessage) {
    if ((cmpt != NULL) && (rxMessage != NULL)) {

        // make sure this is a query global pose message
        if (rxMessage->commandCode == JAUS_QUERY_GLOBAL_POSE) {

            // decode query message
            QueryGlobalPoseMessage queryGlobalPose =
                queryGlobalPoseMessageFromJausMessage(rxMessage);
            if (queryGlobalPose != NULL) {

                // create report message
                ReportGlobalPoseMessage reportGlobalPose =
                    reportGlobalPoseMessageCreate();

                // initialize report message
                jausAddressCopy(reportGlobalPose->destination,
                    queryGlobalPose->source);
                reportGlobalPose->presenceVector =
                    queryGlobalPose->presenceVector;
                reportGlobalPose->sequenceNumber = 0;
                reportGlobalPose->properties.scFlag = 0;

                // fill in report message
                fillInGlobalPose(reportGlobalPose);

                // send report message
                JausMessage txMessage =
                    reportGlobalPoseMessageToJausMessage(reportGlobalPose);
                ojCmptSendMessage(cmpt, txMessage);
                jausMessageDestroy(txMessage);

                reportGlobalPoseMessageDestroy(reportGlobalPose);
                queryGlobalPoseMessageDestroy(queryGlobalPose);
            }
        } else {
            ojCmptDefaultMessageProcessor(cmpt, rxMessage);
        }
    }
}

void NmeaOpenJausComponent::processState(OjCmpt cmpt) {
    if (cmpt != NULL) {

        // check for active service connections
        if (ojCmptIsOutgoingScActive(cmpt, JAUS_REPORT_GLOBAL_POSE)) {
            ServiceConnection scList =
                ojCmptGetScSendList(cmpt, JAUS_REPORT_GLOBAL_POSE);
            if (scList != NULL) {

                // create report message
                ReportGlobalPoseMessage reportGlobalPose =
                    reportGlobalPoseMessageCreate();

                // loop thru pending service connections
                ServiceConnection sc = scList;
                while (sc != NULL) {

                    // initialize report message
                    jausAddressCopy(reportGlobalPose->destination, sc->address);
                    reportGlobalPose->presenceVector =
                        (JausUnsignedShort) sc->presenceVector;
                    reportGlobalPose->sequenceNumber = sc->sequenceNumber;
                    reportGlobalPose->properties.scFlag =
                        JAUS_SERVICE_CONNECTION_MESSAGE;

                    // fill in report message
                    fillInGlobalPose(reportGlobalPose);

                    // send report message
                    JausMessage txMessage =
                        reportGlobalPoseMessageToJausMessage(reportGlobalPose);
                    ojCmptSendMessage(cmpt, txMessage);
                    jausMessageDestroy(txMessage);

                    sc = sc->nextSc;
                }
                reportGlobalPoseMessageDestroy(reportGlobalPose);
            }
            ojCmptDestroySendList(scList);
        }
    }
}

void NmeaOpenJausComponent::fillInGlobalPose(
        ReportGlobalPoseMessage reportGlobalPose) {
    if ((reportGlobalPose != NULL) &&
        (nmeaInterface    != NULL) &&
        (nmeaStatus       != NULL)) {

        // get most recent nmea status
        nmeaInterface->getStaus(nmeaStatus);

        // fill in global pose from nmea status
        reportGlobalPose->latitudeDegrees       = nmeaStatus->latitude_degN;
        reportGlobalPose->longitudeDegrees      = nmeaStatus->longitude_degE;
        reportGlobalPose->elevationMeters       = nmeaStatus->altitude_m;
        reportGlobalPose->positionRmsMeters     = 0.0;  // TODO
        reportGlobalPose->rollRadians           = 0.0;
        reportGlobalPose->pitchRadians          = 0.0;
        reportGlobalPose->yawRadians            = 0.0;
        reportGlobalPose->attitudeRmsRadians    = 0.0;

        time_t seconds = static_cast<time_t> (floor(nmeaStatus->time_s));
        JausUnsignedShort millisec = static_cast<JausUnsignedShort>
            (floor((nmeaStatus->time_s - seconds) * 1000));
        if (!jausTimeSetTime(reportGlobalPose->time, seconds, millisec)) {
            jausUnsignedShortClearBit(&reportGlobalPose->presenceVector,
                JAUS_POSE_PV_TIME_STAMP_BIT);
        }

        jausUnsignedShortClearBit(&reportGlobalPose->presenceVector,
            JAUS_POSE_PV_POSITION_RMS_BIT);
        jausUnsignedShortClearBit(&reportGlobalPose->presenceVector,
            JAUS_POSE_PV_ROLL_BIT);
        jausUnsignedShortClearBit(&reportGlobalPose->presenceVector,
            JAUS_POSE_PV_PITCH_BIT);
        jausUnsignedShortClearBit(&reportGlobalPose->presenceVector,
            JAUS_POSE_PV_YAW_BIT);
        jausUnsignedShortClearBit(&reportGlobalPose->presenceVector,
            JAUS_POSE_PV_ATTITUDE_RMS_BIT);
    }
}

JausBoolean jausTimeSetTime(JausTime jausTime, time_t timeVal,
                            JausUnsignedShort millisec) {
    JausBoolean flag = jausTimeSetTime(jausTime, timeVal);

    if (flag) {
        jausTime->millisec = millisec;
        jausTime->timeStamp = jausTimeGetTimeStamp(jausTime);
    }

    return flag;
}
